#include "StdAfx.h"
#include "Env.h"
#include "CarAgent.h"
#include "TaxiAgent.h"
#include "Utils.h"
#include "math.h"
#include <algorithm>

#define NUM_COLORS  5
static COLORREF c_pathColors[NUM_COLORS] = {
	RGB(255, 255, 0), //yellow
	RGB(255, 128, 0), //orange
	RGB(0, 128, 0),   //green
	RGB(255, 0, 0),   //red
	RGB(0, 0, 128)    //blue
};

CEnv::CEnv(const CString& mapfile)
{
    m_strDesc = m_map.LoadMapFile(mapfile);
    m_nextAgentId = 0;
    m_numCars = 0;
    m_pps = 100;
    m_nextColor = 0;
    SpawnFares();
}

CEnv::~CEnv(void)
{
    for (CAgentPtrVecIter it = m_agents.begin(); it != m_agents.end(); ++it)
    {
        if (*it) delete *it;
    }

    for(CFarePtrVecIter it = m_vecFares.begin(); it != m_vecFares.end(); ++it)
	{
		if (*it) delete *it;
	}
}

void CEnv::SpawnCarAgent()
{
    CNode* src;
    CNode* dest;
    int rand = CAgentUtils::Random(0, m_map.m_vecInlets.size());

    src = m_map.m_vecInlets[rand];

    do
    {
        rand = CAgentUtils::Random(0, m_map.m_vecOutlets.size());
        dest = m_map.m_vecOutlets[rand];
    }
    while (dest->m_id == src->m_id);

    CPath* path = CPath::FindPath(src->m_id, dest->m_id, m_map.m_vecPaths);

    CCarAgent* agent = new CCarAgent(
        m_nextAgentId++, 
        path);

    m_agents.insert(m_agents.begin(), (CAgent*)agent);
    m_numCars++;
}

void CEnv::SpawnTaxiAgent()
{
    CTaxiAgent* agent = new CTaxiAgent(m_nextAgentId++, GetRandomNode(), 
        c_pathColors[m_nextColor++], this);

    if (m_nextColor == NUM_COLORS)
        m_nextColor = 0;

    m_agents.push_back((CAgent*)agent);
}

void CEnv::Step(int ms)
{
    for (CNodePtrVecIter it = m_map.m_vecNodes.begin();
        it != m_map.m_vecNodes.end(); ++it)
    {
        (*it)->m_trafE = 0;
        (*it)->m_trafW = 0;
        (*it)->m_trafN = 0;
        (*it)->m_trafS = 0;
    }

    UpdateAgents(ms);
    UpdateFares(ms);


    for (CAgentPtrVecIter it = m_agents.begin(); it != m_agents.end(); ++it)
    {

    }
}

void CEnv::UpdateAgents(int ms)
{
    CAgentPtrVec vecRemove;

    for (CAgentPtrVecIter it = m_agents.begin(); it != m_agents.end(); ++it)
    {
        CAgent* agent = *it;
        bool ret = agent->Step(this, ms);

        if (agent->GetType() == ETYPE_CAR)
        {
            if (!ret)
            {
                vecRemove.push_back(agent);
                m_numCars--;
            }
        }
        else if (agent->GetType() == ETYPE_TAXI)
        {
        }
        else
        {
            TRACE("Invalid or unknown agent type encountered in %s\n", __FUNCTION__);
            ASSERT(FALSE);
        }
    }

    for (CAgentPtrVecIter it = vecRemove.begin(); it != vecRemove.end(); ++it)
    {
        CAgentPtrVecIter itFind = std::find(m_agents.begin(), m_agents.end(), *it);
        if (itFind != m_agents.end())
        {
            delete *itFind;
            m_agents.erase(itFind);
        }
        else
        {
            TRACE("Could not delete agent");
            ASSERT(FALSE);
        }
    }
}

void CEnv::UpdateFares(int ms)
{
    CFarePtrVec vecRemove;
    for (CFarePtrVecIter it = m_vecFares.begin(); it != m_vecFares.end(); ++it)
	{
		CFare* fare = *it;
		if(!fare->GetInProgress() && fare->GetExpire() <= CSysTime::Now())
		{
			fare->m_start->m_numFares--;
			vecRemove.push_back(*it);
		}
	}

	for (CFarePtrVecIter it = vecRemove.begin(); it != vecRemove.end(); ++it)
    {
        CFarePtrVecIter itFind = std::find(m_vecFares.begin(), m_vecFares.end(), *it);
        if (itFind != m_vecFares.end())
        {
            delete *itFind;
            m_vecFares.erase(itFind);
        }
        else
        {
            TRACE("Could not delete fare");
            ASSERT(FALSE);
        }
    }

	if(m_vecFares.size() < MAX_FARES)
	{
        CNode* start = GetRandomNode();
        CNode* end;
        do
        {
		    end = GetRandomNode();
        }
        while (start == end);

        m_vecFares.push_back(new CFare(m_vecFares.size(), start, end));
	}
}

//double CEnv::GetNodeTraffic(CNode* from, CNode* to)
//{
//    if (!from || !to)
//        return 0;
//
//    int numCars = 0;
//
//    //for (CAgentPtrVecIter it = m_agents.begin(); it != m_agents.end(); ++it)
//    //{
//    //    CAgent* agent = *it;
//    //    if (agent->m_node->m_id == from->m_id
//    //        && agent->m_next->m_id == 
//    //}
//
//
//    for (CAgentPtrVecCIter it = from->m_agents.begin();
//        it != from->m_agents.end(); ++it)
//    {
//        const CAgent* agent = *it;
//        if (agent->m_next && agent->m_next->m_id == to->m_id)
//        {
//            numCars++;
//        }
//    }
//
//    int distance = from->GetDistance(to);
//    double capacity = (double)distance / 8.0;
//
//    return capacity / numCars;
//}
//
//void CEnv::UpdateTraffic()
//{
//    for (CNodePtrVecIter it = m_map.m_vecNodes.begin();
//        it != m_map.m_vecNodes.end(); ++it)
//    {
//        CNode* n = *it;
//        n->m_trafN = GetNodeTraffic(n, n->m_north);
//        n->m_trafE = GetNodeTraffic(n, n->m_east);
//        n->m_trafS = GetNodeTraffic(n, n->m_south);
//        n->m_trafW = GetNodeTraffic(n, n->m_west);
//    }
//}

void CEnv::SpawnFares()
{
    //for (int i = 0; i < MAX_FARES; i++)
    //{
    //    CNode* start = GetRandomNode();
    //    CNode* end = NULL;
    //    do 
    //    {
    //        end = GetRandomNode();
    //    }
    //    while (end == start);
    //    // start->m_numFares++; This is done in the CFare constructor now. You
    //    // Can delete this comment and the code when you get here.
    //    m_vecFares.push_back(new CFare(start, end));
    //}
}

CFare* CEnv::GetClosestFare(CNode* node)
{
    CFare* fare = NULL;
    for(CFarePtrVecIter it = m_vecFares.begin(); it != m_vecFares.end(); ++it)
    {
        CFare* cfare = *it;

        if (cfare->GetInProgress())
            continue;

        if (fare == NULL)
        {
            fare = cfare;
            continue;
        }

        int d1 = GetNodeDist(cfare->m_start, node);
        int d2 = GetNodeDist(fare->m_start, node);
        if (d1 < d2)
        {
            fare = cfare;
        }
    }

    return fare;
}

int CEnv::GetNodeDist(CNode* n1, CNode* n2)
{
    CPoint& pt1 = n1->m_pt;
    CPoint& pt2 = n2->m_pt;

    return (int) sqrt( (double)((pt1.x - pt2.x)*(pt1.x - pt2.x) 
                              + (pt1.y - pt2.y)*(pt1.y - pt2.y) ));
}

CNode* CEnv::GetRandomNode()
{
    int indx = CAgentUtils::Random(0, m_map.m_vecNodes.size());
    return m_map.m_vecNodes[indx];
}

CNodeMap* CEnv::GetNodeMap()
{
    return &m_map;
}

CNodePtrVec* CEnv::GetNodes()
{
    return &m_map.m_vecNodes;
}

CAgentPtrVec* CEnv::GetAgents()
{
    return &m_agents;
}

CFarePtrVec* CEnv::GetFares()
{
    return &m_vecFares;
}

CString CEnv::GetMapDesc()
{
    return m_strDesc;
}

int CEnv::GetNumCars()
{
    return m_numCars;
}

int CEnv::GetAgentSpeed()
{
    return m_pps;
}   

void CEnv::SetAgentSpeed(int val)
{
    m_pps = val;
}